#!/usr/bin/env python

import euclid
import math
import game.gameworld
import game.entity
import random
import geom
import path

#-------------------------------------------------
class Behaviour(object):
    def __init__( self, priority = 1, weight = 1 ):
        self.weight = weight
        self.priority = priority
        
    def __str__(self):
        raise NotImplementedError()
        
    name = __str__
    
    def calculate(self, owner):
        raise NotImplementedError()

#-------------------------------------------------
class Seek( Behaviour ):
    def __init__(self, target, priority = 1, weight = 1):
        super( Seek, self ).__init__( priority, weight )
        self.target = target
        
    def __str__(self):
        return "seek"
    
    def calculate( self, owner ):
        return _seek( self.target, owner )
 
#-------------------------------------------------       
class Flee( Behaviour ):
    def __init__(self, target, radius = 0, priority = 1, weight = 1):
        super( Flee, self ).__init__( priority, weight )
        self.target = target
        self.radius = radius
        
    def __str__(self):
        return "flee"
    
    def calculate( self, owner ):
        totarget = (owner.pos - self.target )
        if ( self.radius > 0 and totarget.magnitude_squared() < self.radius**2 ) or self.radius == 0 :
            return _flee( self.target, owner )
        else:
            return euclid.Vector3()

#-------------------------------------------------
class Arrive( Behaviour ):
    def __init__(self, target, deceleration, priority = 1, weight = 1):
        super( Arrive, self ).__init__( priority, weight )
        self.target = target
        self.deceleration = deceleration
        
        assert self.deceleration > 0, "deceleration must be positive"
        
    def __str__(self):
        return "arrive"
    
    def calculate(self, owner):
        return _arrive( self.target, owner, self.deceleration )
    
#-------------------------------------------------
class Pursuit(Behaviour):
    def __init__(self, target, priority = 1, weight = 1):
        super( Pursuit, self ).__init__( priority, weight )
        self.target = target
        
        assert isinstance( self.target, game.entity.MovingEntity )
        
    def __str__(self):
        return "pursuit"
    
    def calculate(self, owner):
        toTarget = self.target.pos - owner.pos
        relativeHeading = owner.heading.dot( self.target.heading )
        
        if toTarget.dot( owner.heading ) > 0 and relativeHeading < -0.95: #arccos( 0.95 ) = 18 degs
            return _seek( self.target.pos, owner )
        else:
            lookAhead = float( toTarget.magnitude() ) / float( owner.maxSpeed + self.target.velocity.magnitude() )
            lookAhead += _turnAroundTime( owner, self.target.pos )
            
            return _seek( self.target.pos + ( self.target.velocity * lookAhead), owner )
        

#-------------------------------------------------
class Evade(Behaviour):
    def __init__(self, target, priority = 1, weight = 1):
        super( Evade, self ).__init__( priority, weight )
        self.target = target
        
        assert isinstance( self.target, game.entity.MovingEntity )
        
    def __str__(self):
        return "evade"
    
    def calculate( self, owner ):
        return _evade( self.target, owner )
    

#-------------------------------------------------
class Wander( Behaviour ):

    TARGET_SIZE = 10

    def __init__( self, radius, distance, jitter, priority = 1, weight = 1 ):
        super( Wander, self ).__init__( priority, weight )
        self.radius = radius
        self.distance = distance
        self.jitter = jitter
        self.target  = None
        
    def __str__(self):
        return "wander"
    
    def calculate(self, owner):
        if not self.target or self._arrived( owner ) :
            self.target = self._getTarget( owner )
        
        return _seek( self.target, owner )

    def _arrived(self, owner):
        return ( self.target - owner.pos ).magnitude_squared() < self.TARGET_SIZE ** 2

    def _getTarget(self, owner):
        #Init the target to a random position on the circle
        theta = random.random() * (math.pi * 2)

        #Must be point for the matrix multiplication to work
        target = euclid.Point3( self.radius * math.cos( theta ), self.radius * math.sin( theta ) )

        #Add jitter to the target
        #target += euclid.Vector3( random.uniform( -1, 1 ) * self.jitter,
        #    random.uniform( -1, 1 ) * self.jitter)
        target.normalize()
        target *= self.radius

        target = target + euclid.Vector3( self.distance, 0, 0)
        return owner.transform * target

#-------------------------------------------------
class Interpose( Behaviour ):
    def __init__(self, target1, target2, priority = 1, weight = 1):
        assert isinstance(target1, game.entity.GameEntity) and isinstance( target2, game.entity.GameEntity )
        super(Interpose, self).__init__( priority, weight )
        
        self.target1 = target1
        self.target2 = target2

    def __str__( self ):
        return "interpose"
    
    def calculate( self, owner ):
        midpoint = ( self.target1.pos + self.target2.pos ) / 2.0
        timeToMidPoint = ( owner.pos - midpoint ).magnitude() / owner.maxSpeed
        
        #calculate future positions
        pos1 = self.target1.pos + ( self.target1.velocity * timeToMidPoint )
        pos2 = self.target2.pos + ( self.target2.velocity * timeToMidPoint )
        
        midpoint = ( pos1 + pos2 ) / 2
        return _arrive( midpoint, owner, Speed.FAST )

#-------------------------------------------------
class OffsetPursuit( Behaviour ):
    def __init__( self, target, offset, priority = 1, weight = 1 ):
        assert isinstance( target, game.entity.GameEntity )
        assert type( offset ) in ( euclid.Vector2, euclid.Vector3, euclid.Point2, euclid.Point3 )
        super( OffsetPursuit, self ).__init__( priority, weight ) 

        #Make sure the offset is a point type for the transformation to work
        if isinstance( offset, euclid.Vector2 ):
            self.offset = euclid.Point2( offset.x, offset.y )
        elif isinstance( offset, euclid.Vector3 ):
            self.offset = euclid.Point3( offset.x, offset.y, offset.z )
        else:
            self.offset = offset
        self.target = target
        
    def __str__( self ):
        return "offsetpursuit"
        
    def calculate( self, owner ):
        worldOffset = self.target.transform * self.offset
        toOffset = worldOffset - owner.pos
            
        lookAhead = toOffset.magnitude() / ( self.target.velocity.magnitude() + owner.maxSpeed )
        return _arrive( worldOffset + ( self.target.velocity * lookAhead ), owner,  Speed.FAST )

#-------------------------------------------------
class Separation( Behaviour ):
    def __init__( self, radius, priority = 1, weight = 1 ):
        super(Separation, self).__init__( priority, weight )
        self.radius = radius
        
    def __str__(self):
        return "separation"
    
    def calculate(self, owner):
        #Grab all moving entities that is not the current owner within a given radius
        neighbours = game.gameworld.instance().getEntities(
            lambda _entity : isinstance( _entity, game.entity.MovingEntity)
            and _entity != owner
            and (_entity.pos - owner.pos ).magnitude() < self.radius )
        
        force = euclid.Vector3()
        for n in neighbours:
            toAgent = owner.pos - n.pos
            if toAgent.magnitude():
                force += toAgent.normalized() / toAgent.magnitude()
        return force
    
#-------------------------------------------------  
class Alignment( Behaviour ):
    def __init__( self, radius, priority = 1, weight = 1 ):
        super(Alignment, self).__init__( priority, weight )
        self.radius = radius
        
    def __str__(self):
        return "alignment"
    
    def calculate(self, owner):
        #Grab all moving entities that is not the current owner within a given radius
        neighbours = game.gameworld.instance().getEntities(
            lambda _entity : isinstance( _entity, game.entity.MovingEntity)
            and _entity != owner
            and (_entity.pos - owner.pos ).magnitude() < self.radius )
        
        heading = euclid.Vector3()
        for n in neighbours:
            heading += n.heading
        
        if len( neighbours ) > 0:
            heading /= float( len( neighbours ) )
            heading -= owner.heading
        
        return heading
  
#-------------------------------------------------  
class Cohesion( Behaviour ):
    def __init__( self, radius, priority = 1, weight = 1 ):
        super(Cohesion, self).__init__( priority, weight )
        self.radius = radius
        
    def __str__(self):
        return "cohesion"
    
    def calculate(self, owner):
        #Grab all moving entities that is not the current owner within a given radius
        neighbours = game.gameworld.instance().getEntities(
            lambda _entity : isinstance( _entity, game.entity.MovingEntity)
            and _entity != owner
            and (_entity.pos - owner.pos ).magnitude() < self.radius )
        
        com = euclid.Vector3() #Center Of Mass
        force = euclid.Vector3()
        
        for n in neighbours:
            com += n.pos
        
        if len( neighbours ) > 0:
            com /= float( len( neighbours ) )
            force = _seek( com, owner )
        return force
      
#-------------------------------------------------  
class FollowPath( Behaviour ):
    def __init__( self, path, priority = 1, weight = 1 ):
        super(FollowPath, self).__init__( priority, weight )
        self.path = path
        
        assert isinstance( self.path, path.Path )
        
    def __str__(self):
        return "followpath"
    
    def calculate(self, owner):
        if self.path.current().arrived( owner.pos ):
            self.path.advance()
        
        if self.path.finished():
            return _arrive( self.path.current().pos, owner, Speed.NORMAL )
        else:
            return _seek( self.path.current().pos, owner )



class ObstacleAvoidance(Behaviour):
    def __init__( self, priority = 1, weight = 1 ):
        super(ObstacleAvoidance, self).__init__( priority, weight )
        self.minDetectionLength = 100

    def __str__(self):
        return "obstacleavoidance"

    def calculate(self, owner):
        _detect = self.minDetectionLength + (owner.velocity.magnitude() / owner.maxSpeed) * self.minDetectionLength

        #Grab all obstacles within viewing distance
        obstacles = game.gameworld.instance().getEntities(
            lambda e : isinstance( e, game.entity.Obstacle ) and (e.pos - owner.pos ).magnitude_squared() < _detect**2 )


        closest = None
        closestPos = None
        minDist = 999999999999999 #A very large number

        for obst in obstacles:
            localPos = owner.transform.inverse() * obst.pos
            if localPos.x >= 0: #Only consider object in front of us
                radius = obst.getBound().radius + owner.getBound().radius
                if abs( localPos.y ) < radius:
                    cX = localPos.x
                    cY = localPos.y
                    sqrtPart = math.sqrt( radius**2 - cY**2 )
                    ip = cX - sqrtPart
                    if ip <= 0:
                        ip = cX + sqrtPart
                    if ip < minDist:
                        minDist = ip
                        closest = obst
                        closestPos = localPos

        steer = euclid.Vector3()
        if closest:
            factor = 1.0 + ( _detect - closestPos.x ) / _detect
            steer.y = (closest.getBound().radius - closestPos.y) * factor

            breakFactor = 0.2
            steer.x = ( closest.getBound().radius - closestPos.x ) * breakFactor

        return owner.transform * steer

class Hide(Behaviour):
    minDetectionLength = 100
    minDistanceToTarget = 30


    def __init__( self, target, priority = 1, weight = 1 ):
        super(Hide, self).__init__( priority, weight )
        self.target = target

    def __str__(self):
        return "hide"
    
    def calculate(self, owner):
        _detect = self.minDetectionLength + (owner.velocity.magnitude() / owner.maxSpeed) * self.minDetectionLength
        #Grab all obstacles within viewing distance
        obstacles = game.gameworld.instance().getEntities(
            lambda e : isinstance( e, game.entity.Obstacle ) and (e.pos - owner.pos ).magnitude_squared() < _detect**2 )

        bestSpot = None
        distToSpot = None

        for obst in obstacles:
            hidingSpot = self._getHidingPos( obst.pos, obst.getBound().radius, self.target.pos )
            dist = (owner.pos - hidingSpot).magnitude_squared()
            if distToSpot is None or dist < distToSpot:
                bestSpot = hidingSpot
                distToSpot = dist

        if bestSpot:
            return _arrive( bestSpot, owner, Speed.FAST )
        else:
            return _evade( self.target, owner )

    def _getHidingPos(self, obstPos, obstRadius, targetPos):
        """Find a position on the direct opposite side of an obstacle from the target
        Make sure that the position is at least minDistanceToTarget away from the obstacle"""
        dist = obstRadius + self.minDistanceToTarget
        toObst = (obstPos - targetPos).normalized()
        return obstPos + ( toObst * dist )



def _evade(target, _entity):
    toPursuer = target.pos - _entity.pos
    lookAhead = toPursuer.magnitude() / ( _entity.maxSpeed + target.velocity.magnitude() )
    return _flee( target.pos + ( target.velocity * lookAhead ), _entity )
#-------------------------------------------------
def _seek( pos, _entity ):
    assert isinstance( _entity, game.entity.MovingEntity )
    assert type( pos ) in ( euclid.Vector2, euclid.Vector3, euclid.Point2, euclid.Point3 )
    
    targetVelocity = (pos - _entity.pos).normalize() * _entity.maxSpeed
    return targetVelocity - _entity.velocity

#-------------------------------------------------
def _flee( pos, _entity ):
    assert isinstance( _entity, game.entity.MovingEntity )
    assert type( pos ) in ( euclid.Vector2, euclid.Vector3, euclid.Point2, euclid.Point3 )
    
    targetVelocity = (_entity.pos - pos ).normalize() * _entity.maxSpeed
    return targetVelocity - _entity.velocity

#-------------------------------------------------
class Speed( object ):
    __slots__ = [ 'FAST', 'NORMAL', 'SLOW' ]
    FAST = 0.3
    NORMAL = 0.6
    SLOW = 0.9

#-------------------------------------------------
def _arrive( pos, _entity, speed_factor ):
    assert isinstance( _entity, game.entity.MovingEntity )
    assert type( pos ) in ( euclid.Vector2, euclid.Vector3, euclid.Point2, euclid.Point3 )
    
    toTarget = pos - _entity.pos
    dist = toTarget.magnitude()
    if dist > 0:
        speed = min( dist / speed_factor, _entity.maxSpeed)
        targetVelocity = toTarget * ( speed / dist )
        return targetVelocity - _entity.velocity
        
    else:
        return euclid.Vector3(  )

#-------------------------------------------------
def _turnAroundTime( _entity, targetPos ):
    assert isinstance( _entity, game.entity.MovingEntity )
    assert type( targetPos ) in ( euclid.Vector2, euclid.Vector3, euclid.Point2, euclid.Point3 )
    
    dot = _entity.heading.dot( targetPos )
    coefficient = float(1) / float(_entity.maxTurnRate)
    return ( dot - 1 ) * coefficient

#-------------------------------------------------
class SteerEngine(object):
    def __init__(self, owner):
        self.behaviours = []
        self.owner = owner
    
    def clear(self):
        self.behaviours = []
        
    def add(self, behaviour):
        def _findFirstOf( list, behaviour ):
            for i, b in enumerate( list ):
                if b.__str__() == behaviour.__str__():
                    return i
            return -1
        
        #Make sure we only have at most one instance of each behaviour at any given time
        # Is there a more pythonic way of doing this??
        index = _findFirstOf( self.behaviours, behaviour )
        if index != -1:
            del self.behaviours[index]

        self.behaviours.append(  behaviour )
        
    
    def remove( self, behaviour ):
        if isinstance( behaviour, str ):
            self.behaviours = [ b for b in self.behaviours if b.name() != behaviour ]
        elif isinstance( behaviour, Behaviour ):
            try:
                self.behaviours.remove( behaviour )
            except KeyError:
                pass
    
    def _weightedSum(self):
        force = euclid.Vector3()
        for b in self.behaviours:
            force += ( b.calculate( self.owner )  * b.weight )
        return geom.algebra.truncate(force, self.owner.maxForce )
        
    def _truncatedRunningSum(self):
        self.behaviours = sorted( self.behaviours, key = lambda b : b.priority )
        
        total = euclid.Vector3()
        
        def _accumulateForce( _total, toAdd ):
            assert type( toAdd ) in ( euclid.Vector2, euclid.Vector3 )
            
            added = _total.magnitude()
            remaining = self.owner.maxForce - added
            
            if remaining <= 0:
                return euclid.Vector3(), True
            
            forceToAdd = toAdd.magnitude()
            if forceToAdd < remaining:
                return _total + toAdd, False
            else:
                return _total + toAdd.normalized() * remaining, True
        
        for b in self.behaviours:
            total, finished = _accumulateForce( total, b.calculate( self.owner ) * b.weight )
            if finished:
                break

        return total
            
    def _prioritizedDithered(self):
        self.behaviours = sorted( self.behaviours, key = lambda b : b.priority )
        for b in self.behaviours:
            if random.random() < b.priority:
                force = b.calculate( self.owner ) * (b.weight / b.priority)
                if force:
                    return force
        
    calculate = _truncatedRunningSum
    
#TODO: __all__ = []